/*
TP Peripherique
Copyright 2009 Boris Maquin, Paul Cox 

File: main.c
Version: 2
*/

#include <intrins.h>	// pour la fonction void _nop_(void)
#include <reg167.h>
#include <stdio.h>	
#include "../lib/moteur_plateau.h"
#include "../lib/radio_command.h"
#include "../lib/swuart.h"
#include "../lib/Girouette.h"
#include "../lib/servo_moteur.h"
#include "../lib/accelerometre.h"

#define MOT_SEUIL  (1)

void init_fonction();
void Init_timer1();

int speed1;
int dir1;
int angle_girouette;
int compteur=0;

static int vit_eff=0;

int abs(int val) {
	if (val < 0) {
		return -val;
	}
	return val;
}

void moteur_slew()  {
	int vit_des,dif;

	vit_des = get_speed();
	dif = vit_des - vit_eff;
	if ( abs(dif) < MOT_SEUIL ){
		vit_eff = vit_des;
	} else {
		if (dif > 0) {
			dif = MOT_SEUIL;
		} else {
			dif = -MOT_SEUIL;
		}
	vit_eff += dif;
	}	
	gerer_moteur_plateau(vit_eff,get_direction());
}

void Attendons1 (long int Duree)	{

	long Compte;

	Compte = Duree ;
	while (Compte > 0 ){
		_nop_();
		Compte = Compte-1;
	}
}

void main() {

	//char* pchar = "o";

 	init_fonction();
	IEN = 1;		  // autorisation globale des interruptions
			
	//printf("hello \n");
	while (1){
//		printf("Speed: %d Dir: %d\r\n",get_speed() , get_direction());
//		gerer_moteur_plateau( get_speed() , get_direction() );

///		moteur_slew();
//		Attendons1(0xF504);
				
		//put_char1('5');
		_nop_();
/*		printf("Yahoooo!\n");
			put_char1('B');
			put_char1('B');
			put_char1('B');
			put_char1('B');
			put_char1('B');

		}*/
	}
} 

void init_fonction() {
	init_swuart();
	init_rc();
	init_moteur_plateau();
	T2CON = 0x00;
	init_girouette();
	config_interrupt(1,0);
	init_servo_moteur();
	config_accelerometre();

	// Tant qu'on n'a pas vu l'index, on attend  
	while (~index_vu())  
	{
		_nop_();
	}
	 
	// Maintenant qu'on a vu l'index, on peut calculer l'angle du vent, regler la voile,....
	// On va utiliser un compteur (Timer1), qui va declencher une interruption toutes les 10 ms 
	
	
	// On configure et initialise le timer T1
	Init_timer1();
}

void Interupcion_compteur1 (void) interrupt 0x21
{
	int penche1;

	valeur_angle();

 	if ( penche_trop() ) {
  		detendre_voiles();
		penche1=1;
		//put_char1('B');
	} else {
	    penche1=0;
	}


	if ( ~((compteur) % 10 ) )
	{
	 	moteur_slew();
	}

	if ( ~(compteur % 75) )
	{
		angle_girouette = retourne_angle_degres(); 
   		if (penche1==0) {
			reglage_ecoute( conversion_angle_girouette( angle_girouette ) );
		}
	}
/*
	if ( ~(compteur % 100) )
	{
		read_bat_level();	
	}
*/
	if ( compteur < 300 ) {
		compteur++;
	} else	{
		compteur = 0;
	}
						      				 
}

void Init_timer1(void)
{

// On utilise le timer T1, donc on configure le registre T01CON :

	T01CON&=0x00FF;		   // les 8 bits de pf controlent le Timer T0 et les 8 autres le Timer T1
						   // On fait un masque avec FF pour T0, pour pas le modifier, 00 pour T1
						   // pour l'initialiser a 0

	T01CON|=(1<<14);
	    //    T1R=1	    T1I=000
	    //    run       


	T1IE=1;

 	// On initialise le registre T1IC :
 	T1IC &= ~0x3F  ;  // pour initialiser les 6 dernier bits a 0, sans toucher au 7e bit
	T1IC |= (2<<2) ;  // ILVL = 2  (priorite de l'interruption)

	// On met notre valeur du RELAOD
	T1REL=65535.0-(25000.0+1);
}
		